#include "interface.h"
#include "imu.h"
#include "mahony.h"
Interface_gimbal_to_chassis gimbal_to_chassis={0};
Interface_power_to_chassis power_to_chassis={0};
Interface_Quaternion_gimbal q_to_gimbal={30000,0,0,0};

void Quaternion_gimbal_cal(Interface_Quaternion_gimbal* q,Mahony_TypeDef* m)
{
	q->q0=(int16_t)(30000.0f*m->q0);
	q->q1=(int16_t)(30000.0f*m->q1);
	q->q2=(int16_t)(30000.0f*m->q2);
	q->q3=(int16_t)(30000.0f*m->q3);
}

uint8_t interface_Quaternion_cmd(CAN_HandleTypeDef *hcan,Interface_Quaternion_gimbal* q)
{
	uint8_t mode = 1;
	uint8_t data[8] = {0};
	data[0] = q->q0;
	data[1] = q->q0 >> 8 ;
	data[2] = q->q1;
	data[3] = q->q1 >> 8 ;
	data[4] = q->q2;
	data[5] = q->q2 >> 8 ;
	data[6] = q->q3;
	data[7] = q->q3 >> 8;
	uint32_t CAN_TX_BOX0;
	CAN_TxHeaderTypeDef tx_header;
	tx_header.StdId = 0x123;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 8;
	tx_header.TransmitGlobalTime = DISABLE;
	mode = HAL_CAN_AddTxMessage(hcan, &tx_header, data, &CAN_TX_BOX0);
	return mode;
}

void interface_callback(CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
	if (rx_header->StdId == 0x124)
	{
		gimbal_to_chassis.Vx = data[0] | (data[1] << 8);
		gimbal_to_chassis.Vy = data[2] | (data[3] << 8);
		gimbal_to_chassis.Vw = data[4] | (data[5] << 8);
		gimbal_to_chassis.power_percentage = data[6] | (data[7] << 8);
		//HAL_IWDG_Refresh(&hiwdg);
	}
	
	if (rx_header->StdId == 0x212)
	{
		power_to_chassis.I = data[0] | (data[1] << 8);
		power_to_chassis.V = data[2] | (data[3] << 8);
	}
	
	if (rx_header->StdId == 0x123)
	{
		q_to_gimbal.q0 = data[0] | (data[1] << 8);
		q_to_gimbal.q1 = data[2] | (data[3] << 8);
		q_to_gimbal.q2 = data[4] | (data[5] << 8);
		q_to_gimbal.q3 = data[6] | (data[7] << 8);
	}
}


